  
#include <stdlib.h> 
#include "std_msgs/msg/u_int32.hpp"
#include "ltme_node/node.h"
rclcpp::Node::SharedPtr node;
namespace ping_node
{
 	class Pingnode
	{
	public:
	uint32_t toplaser_commu_info;
	uint32_t leftlaser_commu_info;
	uint32_t rightlaser_commu_info;
	std::string agv_type; 
	rclcpp::Publisher<std_msgs::msg::UInt32>::SharedPtr toplaser_com_publisher;
	rclcpp::Publisher<std_msgs::msg::UInt32>::SharedPtr leftlaser_com_publisher;
	rclcpp::Publisher<std_msgs::msg::UInt32>::SharedPtr rightlaser_com_publisher;
	rclcpp::TimerBase::SharedPtr double_timer;
	rclcpp::TimerBase::SharedPtr top_timer;
	void Double_laser_Timer_Callback();
	void Top_laser_Timer_Callback();
	void Init(void);
	};


void Pingnode::Double_laser_Timer_Callback()
{
	std::string leftAddressStr="ping -W 0.4 -c 1 ";
	std::string rightAddressStr="ping -W 0.4 -c 1 ";
	
	std::string left_rightAddressStrTwi="ping -W 0.02 -c 1 ";
	
	const char* ipleftAddress = "172.31.255.13"; // 左雷达 
	const char* iprightAddress = "172.31.255.14"; // 右雷达	
	
	//printf("---DOUBLE:%lf----\r\n");
	//printf("---DOUBLE:%lf----\r\n");
	leftAddressStr += ipleftAddress;
	rightAddressStr += iprightAddress;
	
	FILE *lpp = popen(leftAddressStr.c_str(), "r");
    int lret = pclose(lpp);
	
	FILE *rpp = popen(rightAddressStr.c_str(), "r");
    int rret = pclose(rpp);
	
	if(WEXITSTATUS(lret) == 0) //左雷达
    {
		std_msgs::msg::UInt32 leftlasercom;
		uint32_t leftlaser_commu_info = 0;//正常
		leftlasercom.data = leftlaser_commu_info;
		leftlaser_com_publisher->publish(leftlasercom);
		//测试用
		//auto now = rclcpp::Clock().now().seconds(); 
		//printf("----左正常:%lf----\r\n", now);
		//测试用
    }
    else
    {
		left_rightAddressStrTwi += ipleftAddress;
		FILE *lpp = popen(left_rightAddressStrTwi.c_str(), "r");
		int lret = pclose(lpp);
		
		if (WEXITSTATUS(lret) == 0)
		{
		std_msgs::msg::UInt32 leftlasercom;
		uint32_t leftlaser_commu_info = 0;//正常
		leftlasercom.data = leftlaser_commu_info;
		leftlaser_com_publisher->publish(leftlasercom);	
		}
		else
		{
		//printf("---运行到左2次ping的位置了----\r\n");
		std_msgs::msg::UInt32 leftlasercom;
		uint32_t leftlaser_commu_info = 1;//不正常
		leftlasercom.data = leftlaser_commu_info;
		leftlaser_com_publisher->publish(leftlasercom);
		//测试用
		//auto now = rclcpp::Clock().now().seconds(); 
		//printf("----左不正常:%lf----\r\n", now);
		//测试用
		}
    }
	
	if(WEXITSTATUS(rret) == 0) //右雷达
    {
		std_msgs::msg::UInt32 rightlasercom;
		uint32_t rightlaser_commu_info = 0;//正常
		rightlasercom.data = rightlaser_commu_info;
		rightlaser_com_publisher->publish(rightlasercom);
		//测试用	
		//auto now = rclcpp::Clock().now().seconds(); 
		//printf("----右正常:%lf----\r\n", now);
		//测试用
    }
    else
    {
		left_rightAddressStrTwi += iprightAddress;
		FILE *rpp = popen(left_rightAddressStrTwi.c_str(), "r");
		int rret = pclose(rpp);
		
		if (WEXITSTATUS(rret) == 0)
		{
		std_msgs::msg::UInt32 rightlasercom;
		uint32_t rightlaser_commu_info = 0;//正常
		rightlasercom.data = rightlaser_commu_info;
		rightlaser_com_publisher->publish(rightlasercom);	
		}
		else
		{
		//printf("---运行到右2次ping的位置了----\r\n");
		std_msgs::msg::UInt32 rightlasercom;
		uint32_t rightlaser_commu_info = 1;//不正常
		rightlasercom.data = rightlaser_commu_info;
		rightlaser_com_publisher->publish(rightlasercom);
		auto now = rclcpp::Clock().now().seconds(); 
		//printf("----右不正常:%lf----\r\n", now);
		}

		//测试用
		//auto now = rclcpp::Clock().now().seconds(); 
		//printf("----右不正常:%lf----\r\n", now);
		//测试用	
    }

	leftlaser_commu_info = 0;
	rightlaser_commu_info = 0;
	
}

void Pingnode::Top_laser_Timer_Callback()
{
	std::string topAddressStr="ping -W 0.4 -c 1 ";
	std::string topAddressStrTwi="ping -W 0.02 -c 1 ";
	
	const char* iptopAddress = "172.31.255.12"; // 顶上的雷达 
	
	topAddressStr += iptopAddress;
	FILE *pp = popen(topAddressStr.c_str(), "r");
    int ret = pclose(pp);
	
    if(WEXITSTATUS(ret) == 0) //顶上的雷达
    {
	uint32_t toplaser_commu_info = 0;
	std_msgs::msg::UInt32 toplasercom;
	toplasercom.data = toplaser_commu_info;
	toplaser_com_publisher->publish(toplasercom);
	auto now = rclcpp::Clock().now().seconds(); 
	//printf("----顶正常:%lf----\r\n", now);
    }
    else   //如果ping不通，再让他ping一次，等待时间0.02s
    {
    topAddressStrTwi += iptopAddress;
	FILE *pp = popen(topAddressStrTwi.c_str(), "r");
    int ret = pclose(pp);
	if (WEXITSTATUS(ret) == 0)
	{
		//printf("---DOUBLE:%lf----\r\n");
		uint32_t toplaser_commu_info = 0;
		std_msgs::msg::UInt32 toplasercom;
		toplasercom.data = toplaser_commu_info;
		toplaser_com_publisher->publish(toplasercom);
	}
	else {
		//printf("---运行到2次ping的位置了----\r\n");
		uint32_t toplaser_commu_info = 1;
		std_msgs::msg::UInt32 toplasercom;
		toplasercom.data = toplaser_commu_info;
		toplaser_com_publisher->publish(toplasercom);
	auto now = rclcpp::Clock().now().seconds(); 
	printf("----顶不正常:%lf----\r\n", now);
	}


    }
	toplaser_commu_info = 0;
}

void Pingnode::Init(void)
{
	  
	node->declare_parameter<std::string>("agv_type","");
	agv_type = node->get_parameter("agv_type").as_string();
	printf("agv_type:%s\n",agv_type.c_str());
	toplaser_com_publisher = node->create_publisher<std_msgs::msg::UInt32>("top_laser_com", 1);
	leftlaser_com_publisher = node->create_publisher<std_msgs::msg::UInt32>("left_laser_com", 1);
	rightlaser_com_publisher = node->create_publisher<std_msgs::msg::UInt32>("right_laser_com", 1);
	
	if(agv_type == "t3000_v1" || agv_type == "t3000_v2" || agv_type == "t3000_v3" || agv_type == "f1200")
	{
	//printf("---进机型判断了----\r\n");
	double_timer  = node->create_wall_timer(std::chrono::milliseconds(int((0.5)*1000)),std::bind(&Pingnode::Double_laser_Timer_Callback, this));
	} 
	top_timer  = node->create_wall_timer(std::chrono::milliseconds(int((0.5)*1000)),std::bind(&Pingnode::Top_laser_Timer_Callback, this));
	
}
}

int main(int argc, char * argv[])
{
	rclcpp::init(argc, argv);
	node =  std::make_shared<rclcpp::Node>("ping_node");

	ping_node::Pingnode Pingnode;
	Pingnode.Init();
	rclcpp::spin(node);
	rclcpp::shutdown(); 
	return 0;
}







